//
// Created by 16933 on 2024/1/13.
//

#include "Calculate.h"
#include "cmsis_os.h"
#include "gpio.h"
#include "tim.h"
#include "Encoder.h"
#include "bsp_MPU6050.h"
#include "Pid.h"
#include "Control.h"
#include "Kalman.h"
#include "FreeRTOS.h"
#define MIDDLE  0.5
int16_t encoder_raw_data = 0;
//motor_measure_t MomentumWheel_Encoder_Handle;
MPU_Data mpu_data;// roll轴数据 左正右负


float mpu_gyro_lowout = 0;
short mpu_angvel_lowout = 0;
short mpu_angvel[3] = {};
float target_angle_speed = 0;

/**-------平衡速度环（速度环）pid参数 PI-------**/
pid_t MomentumWhell_Speed_PID_Controler;
uint32_t pid_maxout_speed = 5;          //输出限幅
uint32_t pid_intergral_limit_speed = 3; //积分限幅
float pid_kp_speed = -0.3;              //KP参数
float pid_ki_speed = -0.02;             //KI参数
float pid_kd_speed = 0;                 //KD参数
uint32_t pid_death_space_speed = 0;
float pid_balance_speed_out = 0;


/**-------直立环（角度环）pid参数 PD-------**/
pid_t MomentumWhell_Balance_PID_Controler;
uint32_t pid_maxout_balance = 2000;          //输出限幅
uint32_t pid_intergral_limit_balance = 500;  //积分限幅
float pid_kp_balance = 200;                  //KP参数
float pid_ki_balance = 0;                    //KI参数
float pid_kd_balance = 950;                  //KD参数
uint32_t pid_death_space_balance = 0;
float pid_balance_out = 0;

/**------角速度环pid参数PI-------**/
pid_t MomentumWheel_AngleSpeed_PID_Controler;
uint32_t pid_maxout_anglespeed = 2000;          //输出限幅
uint32_t pid_intergral_limit_anglespeed = 1000; //积分限幅
float pid_kp_anglespeed = 1.1;                  //KP参数
float pid_ki_anglespeed = 0.022;                //KI参数
float pid_kd_anglespeed = 0;                    //KD参数
uint32_t pid_death_space_anglespeed = 0;        //死区值
float pid_speed_out = 2;

uint8_t pid_cal_sequence = 0;
extern osSemaphoreId ControlBinarySemHandle;
void CalculateTask(void const * argument)
{
    /* USER CODE BEGIN CalculateTask */
    HAL_TIM_Encoder_Start(&htim2,TIM_CHANNEL_ALL); //开启编码器
    __HAL_TIM_SET_COUNTER(&htim2,100);   //编码器测速准备

    mpu_data.interval = 2;

    /** 初始化pid控制器 **/
    //速度外环pid
    pid_init(&MomentumWhell_Speed_PID_Controler,
             pid_maxout_speed,
             pid_intergral_limit_speed,
             pid_kp_speed,
             pid_ki_speed,
             pid_kd_speed,
             pid_death_space_speed);
    //直立环pid
    pid_init(&MomentumWhell_Balance_PID_Controler,
             pid_maxout_balance,
             pid_intergral_limit_balance,
             pid_kp_balance,
             pid_ki_balance,
             pid_kd_balance,
             pid_death_space_balance);

    //角速度环pid
    pid_init(&MomentumWheel_AngleSpeed_PID_Controler,
             pid_maxout_anglespeed,
             pid_intergral_limit_anglespeed,
             pid_kp_anglespeed,
             pid_ki_anglespeed,
             pid_kd_anglespeed,
             pid_death_space_anglespeed);


    xSemaphoreTake(ControlBinarySemHandle,0);
    uint32_t Contiol_time = osKernelSysTick();

    /* Infinite loop */
    for(;;)
    {
        HAL_GPIO_WritePin(TEST_GPIO_Port,TEST_Pin,GPIO_PIN_RESET);
        encoder_raw_data = __HAL_TIM_GET_COUNTER(&htim2);
        MPU_DMP_Get_Data(&mpu_data.gyro_data[0],&mpu_data.gyro_data[1],&mpu_data.gyro_data[2]);

        mpu_data.gyro_low_out = 0.2 * mpu_data.gyro_data[0] + 0.8 * mpu_data.gyro_low_out;

        MPU_Get_Gyroscope(&mpu_angvel[0],&mpu_angvel[1],&mpu_angvel[2]);
        mpu_data.angvel = dmp_gyro_data[1];
        mpu_data.angvel_low_out = mpu_data.angvel;  //本来想滤波 但是从dmp读出来的数据就已经比较好了 没想到好的滤波方法
        encoder_speed_handle(&motor.momentum_weel.encoder_controler,encoder_raw_data);
        if (mpu_data.gyro_low_out > 11 || mpu_data.gyro_low_out < -11 )
        {
            posture = FALLING;
            motor.momentum_weel.pwm_duty = 0;

            //pid参数reset
            pid_reset(&MomentumWhell_Speed_PID_Controler,pid_kp_speed,pid_ki_speed,pid_kd_speed);  //速度环
            pid_reset(&MomentumWhell_Balance_PID_Controler,pid_kp_balance,pid_ki_balance,pid_kd_balance);  //角度环
            // 角度环
            pid_reset(&MomentumWheel_AngleSpeed_PID_Controler,pid_kp_anglespeed,pid_ki_anglespeed,pid_kd_anglespeed); //角速度环
        } else
        {
            posture = BALANCED;
        }
        if(posture == BALANCED)
        {
            if (pid_cal_sequence % 8 == 0)  //16ms算一次速度环
            {

                //速度外环
                pid_balance_speed_out = pid_calc(&MomentumWhell_Speed_PID_Controler,
                                                 motor.momentum_weel.encoder_controler.speed,
                                                 0);
                pid_cal_sequence = 0;
            }
////
            if (pid_cal_sequence % 2 == 0) //4ms算一次角度环
            {
                //角度环
                target_angle_speed = pid_calc(&MomentumWhell_Balance_PID_Controler,
                                                                mpu_data.gyro_low_out,
                                              pid_balance_speed_out);
//                pid_cal_sequence = 0;
            }
            //角速度环 2ms算一次
            pid_speed_out = pid_calc(&MomentumWheel_AngleSpeed_PID_Controler,
                                     mpu_data.angvel_low_out,
                                     target_angle_speed);
            if (pid_speed_out > 0)
            {
                motor.momentum_weel.diraction = CLOCKWISE;
                motor.momentum_weel.pwm_duty = (int)pid_speed_out;
            } else
            {
                motor.momentum_weel.diraction = COUNTERCLOCKWISE;
                motor.momentum_weel.pwm_duty = (int)(-pid_speed_out);
            }
        }

        xSemaphoreGive(ControlBinarySemHandle);//释放信号量
        HAL_GPIO_WritePin(TEST_GPIO_Port,TEST_Pin,GPIO_PIN_SET);
        pid_cal_sequence += 1;
        osDelayUntil(&Contiol_time,2);
    }

    /* USER CODE END CalculateTask */
}